-module(rtps_receiver).

%%% @doc The receiver is handed a full RTPS message from a transport
%%% along with a list of endpoints currently using / connected to the
%%% transport. The receiver takes the message apart and will
%%% distribute the submessages to the appropriate endpoints.

-behaviour(gen_statem).

%% -include_lib("kernel/include/logger.hrl").
-include("rtps.hrl").
-include("rtps_transport.hrl").
-include("rtps_psm.hrl").

%% API
-export([start_link/0, recv/6]).

%% gen_statem callbacks
-export([init/1, callback_mode/0,
	 idle/3,
	 receiving/3, 
	 terminate/3, code_change/4]).

-record(data, {source_version :: rtps:protocol_version() | undefined,
	       source_vendor_id :: rtps:vendor_id() | undefined,
	       source_guid_prefix :: rtps:guid_prefix() | undefined,
	       dest_guid_prefix :: rtps:guid_prefix() | undefined, 
	       unicast_reply_locator_list :: [rtps:loactor()] | undefined,
	       multicast_reply_locator_list :: [rtps:loactor()] | undefined, 
	       unicast_reply_locator_ip4 :: rtps:loactor_udpv4() | undefined,
	       multicast_reply_locator_ip4 :: rtps:loactor_udpv4() | undefined,
	       have_timestamp :: boolean() | undefined, timestamp :: rtps:time() | undefined,
	       endpoints :: [rtps_transport:endpoint()] | undefined,
	       acc :: orddict:orddict() | undefined}).

%%%===================================================================
%%% API
%%%===================================================================

start_link() ->
    gen_statem:start_link(?MODULE, [], []).

-spec recv(Pid, Kind, Address, Port, Message, Endpoints) -> ok when
      Pid :: pid(),
      Kind :: udp | rtps:locator_kind(),
      Address :: inet:ip4_address() | inet:ip6_address(),
      Port :: inet:port_number(),
      Message :: rtps_psm:message(),
      Endpoints :: [rtps_transport:endpoint()].
%% @doc This function is called to forward a message to a receiver.
%%
%% TODO: The `Port' parameter is not used by the receiver and may be
%% removed. It was added to make tracing messages easier.

recv(Pid, udp, Address, Port, Message, Endpoints)
  when tuple_size(Address) =:= 4 ->
    %% ?DEBUG("recv ~p~n", [Message]),
    recv(Pid, udpv4, Address, Port, Message, Endpoints);
recv(Pid, udp, Address, Port, Message, Endpoints)
  when tuple_size(Address) =:= 6 ->
    recv(Pid, udpv6, Address, Port, Message, Endpoints);
recv(Pid, Kind, Address, Port, Message, Endpoints) ->
    gen_statem:call(Pid, {recv, Kind, Address, Port, Message, Endpoints}).

%%%===================================================================
%%% gen_statem callbacks
%%%===================================================================

%% @private
init([]) ->
    {ok, idle, #data{}}.

%% @private
callback_mode() ->
    state_functions.

%% @private

%% @doc 8.3.4 The RTPS Message Receiver. The following function processes the
%% interpreter submessages and sends the submessages to the entities.
%% For each new RTPS message, the state of the Receiver is reset and
%% initialized. See table 8.16 - Initial State of the Receiver.
idle({call, From},
     {recv, Kind, Address, _Port, #message{header = #header{protocol_version = Version,
							    vendor_id = Vendor_id,
							    guid_prefix = Guid_prefix}, 
					   submessages = Submsgs}, Endpoints}, Data) ->
    Uni = #locator{kind = Kind, address = Address, port = invalid},
    Multi = #locator{kind = Kind, address = invalid, port = invalid},
    {next_state, receiving,
     Data#data{source_version = Version,
	       source_vendor_id = Vendor_id,
	       source_guid_prefix = Guid_prefix,
	       dest_guid_prefix = unknown,
	       unicast_reply_locator_list = [Uni], 
	       multicast_reply_locator_list = [Multi], 
	       unicast_reply_locator_ip4 = undefined,
	       multicast_reply_locator_ip4 = undefined,
	       have_timestamp = false, 
	       timestamp = invalid,
	       endpoints = Endpoints,
	       acc = orddict:new()}, % Accumulated messages per destination process
     [{reply, From, ok}, {next_event, internal, Submsgs}]}.

%% 8.3.7.7 InfoDestination
receiving(internal,
	  [#info_destination{guid_prefix = Dst_guid_prefix} | Rest],
	  Data) ->
    {keep_state, Data#data{dest_guid_prefix = Dst_guid_prefix},
     [{next_event, internal, Rest}]};
%% 8.3.7.8 InfoReply (no need for flags here.)
receiving(internal,
	 [#info_reply{multicast_locator_list = M, unicast_locator_list = U} | Rest],
	 Data) ->
    {keep_state, Data#data{unicast_reply_locator_list = U, multicast_reply_locator_list = M},
     [{next_event, internal, Rest}]};
%% TODO: NOWHERE IN THE STANDARD! how this should be interpreted
receiving(internal,
	 [#info_reply_ip4{multicast_locator = M, unicast_locator = U} | Rest],
	 Data) ->
    {keep_state, Data#data{unicast_reply_locator_ip4 = U, multicast_reply_locator_ip4 = M},
     [{next_event, internal, Rest}]};
%% 8.3.7.9 InfoSource
%% TODO: What about the multi-/unicast_reply_locator_ip4 values?
%% Shouldn't they be reset to something, such as invalid. The specs
%% say nothing about this.
receiving(internal,
	 [#info_source{guid_prefix = Guid_prefix, protocol_version = Version, 
		       vendor_id = Vendor_id} | Rest], Data) ->
    {keep_state, Data#data{source_version = Version, source_vendor_id = Vendor_id,
			   source_guid_prefix = Guid_prefix,
			   unicast_reply_locator_list = [invalid], 
			   multicast_reply_locator_list = [invalid], 
			   have_timestamp = false},
     [{next_event, internal, Rest}]};
%% 8.3.7.10 InfoTimestamp
receiving(internal,
	 [#info_timestamp{timestamp = undefined} | Rest],
	 Data) ->
    {keep_state, Data#data{have_timestamp = false},
     [{next_event, internal, Rest}]};
receiving(internal,
	 [#info_timestamp{timestamp = Timestamp} | Rest],
	 Data) ->
    {keep_state, Data#data{have_timestamp = true, timestamp = Timestamp},
     [{next_event, internal, Rest}]};
%% 8.3.7.12 Pad
receiving(internal,
	 [#pad{} | Rest],
	 _Data) ->
   {keep_state_and_data,
    [{next_event, internal, Rest}]};
%% TODO: is it relevant / possible to filter messages at this point
%% based on the destination guid prefix: i.e. if the destination guid
%% prefix is not within the list of guid prefixes of local readers and
%% writers associated with this process, maybe we should just drop
%% them as soon as possible?
receiving(internal,
	 [Submsg | Rest],
	 #data{dest_guid_prefix = Dst_guid_prefix,
	       source_guid_prefix = Src_guid_prefix,
	       unicast_reply_locator_list = UL,
	       multicast_reply_locator_list = ML,
	       unicast_reply_locator_ip4 = UL_ip4,
	       multicast_reply_locator_ip4 = ML_ip4,
	       timestamp = Timestamp, have_timestamp = Have_timestamp,
	       endpoints = Endpoints, acc = Acc} = Data) ->
    case submsg(Submsg) of
	{Dst_entity_id, Src_entity_id} -> 
	    Reply_to = [L || L <- [UL_ip4 | UL] ++ [ML_ip4 | ML], L =/= undefined],
	    TS = [{timestamp, Timestamp}, {have_timestamp, Have_timestamp}],
	    L = distribute(Src_guid_prefix, Src_entity_id, Dst_guid_prefix, Dst_entity_id, Submsg, Reply_to, TS, Endpoints),
	    Acc1 = accum(L, Acc),
	    {keep_state, Data#data{acc = Acc1}, [{next_event, internal, Rest}]};
	ignore ->
	    %% A Submessage with an unknown SubmessageId must be ignored and
	    %% parsing must continue with the next Submessage. See 8.3.4.1 point 3
	    %% (p36).
	    {keep_state_and_data, [{next_event, internal, Rest}]};
	invalid ->
	    %% A submessage is invalid, so stop processing the RTPS
	    %% message as described in 8.3.4.1 Rules Followed by the
	    %% Message Receiver item 6 (p36)
	    {keep_state_and_data, [{next_event, internal, []}]}
    end;
%% Done processing the RTPS message. Now send the accumulated
%% submessages to the endpoints
receiving(internal, [], #data{acc = Acc} = Data) ->
    [Pid ! {rcv, Rcvs} || {Pid, Rcvs} <- orddict:to_list(Acc)],
    {next_state, idle, Data}.

%% @private
terminate(_Reason, _State_name, _Data) ->
    ok.

%% @private
code_change(_Old_vsn, State_name, Data, _Extra) ->
    {ok, State_name, Data}.

%%%===================================================================
%%% Internal functions
%%%===================================================================

%% @private

-spec submsg(Submsg) -> Result when
      Submsg :: rtps_psm:submsg() | undefined,
      Result :: {Dst_entity_id, Src_entity_id} | ignore | invalid,
      Dst_entity_id :: rtps:entity_id(),
      Src_entity_id :: rtps:entity_id().
%% @doc Process the different type of non information submessages.
%%  
%% If a submessage is not valid according to the rules set out in
%% 8.3.7 RTPS Submessages, terminate according to 8.3.4.1 Rules
%% Followed by the Message Receiver item 6 (p36).
submsg(#acknack{reader_sn_state = #sequence_number_set{base = Base}})
  when Base =< 0 ->
    invalid;
submsg(#acknack{writer_id = Dst_entity_id, reader_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
%% TODO: Review validity inline_qos: for what values should be checked?
submsg(#data_msg{writer_sn = Writer_sn, inline_qos = Inline_qos})
  when Writer_sn =< 0; Writer_sn =:= unknown;
       not (is_list(Inline_qos) orelse Inline_qos =:= undefined) ->
    invalid;
submsg(#data_msg{reader_id = Dst_entity_id, writer_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
%% TODO: Can we check on more conditions here?
submsg(#data_frag{writer_sn = Writer_sn, fragment_starting_num = Fragment_starting_num,
		 fragments_in_submessage = Fragments_in_submessage, inline_qos = Inline_qos})
  when Writer_sn =< 0; Writer_sn =:= unknown; Fragment_starting_num =< 0;
       Fragments_in_submessage =< 0;
       not (is_list(Inline_qos) orelse Inline_qos =:= undefined) ->
    invalid;
submsg(#data_frag{reader_id = Dst_entity_id, writer_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
submsg(#gap{gap_start = Gap_start, gap_list = [Base | _]})
  when Gap_start =< 0; Base < Gap_start ->
    invalid;
submsg(#gap{reader_id = Dst_entity_id, writer_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
submsg(#heartbeat{first_sn = First_sn, last_sn = Last_sn})
  when First_sn =< 0; Last_sn =< 0; Last_sn < First_sn ->
    invalid;
submsg(#heartbeat{reader_id = Dst_entity_id, writer_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
submsg(#heartbeat_frag{writer_sn = Writer_sn, last_fragment_num = Last_fragment_num})
  when Writer_sn =< 0; Writer_sn =:= unknown; Last_fragment_num =< 0 ->
    invalid;
submsg(#heartbeat_frag{reader_id = Dst_entity_id, writer_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
submsg(#nack_frag{writer_sn = Writer_sn, fragment_number_state = #fragment_number_set{base = Base}})
  when Writer_sn =< 0; Writer_sn =:= unknown; Base =< 0 ->
    invalid;
submsg(#nack_frag{writer_id = Dst_entity_id, reader_id = Src_entity_id}) ->
    {Dst_entity_id, Src_entity_id};
%% A Submessage with an unknown SubmessageId must be ignored and
%% parsing must continue with the next Submessage. See 8.3.4.1 point 3
%% (p36).
submsg(_) ->
    ignore.

%% @private

%% @doc Submessages are to be distributed to the endpoints based on
%% the destination GUID prefix and destination entity id. In case one
%% of the destination parameters is unknown, submessages go to all
%% endpoints that match partially. The result of this function is a
%% list of tuples, each tuple containing the pid of the endpoint the
%% submessage should go to plus the submsg and related information.
%% 
%% TODO: Review handling looped back data. When the transport has the
%% multicast loopback set, the receiver also gets the RTPS messages
%% being transmitted. To prevent the submessages to be looped back to
%% the producing endpoint, the first function clause with the
%% Dst_guid_prefix =:= Src_guid_prefix guard is used to drop these
%% messages. It would be much nicer if looped back submessages
%% wouldn't get this far because it takes a lot of processing to reach
%% this point.

%% TODO: remove the first clause which drops submessages with the same
%% source and and destination participant. This situation can only be
%% caused by some malfunctioning implementation.
%% distribute(Guid_prefix, _Src_entity_id, Guid_prefix, _Dst_entity_id, _Submsg, _Reply_to, _TS, _Endpoints) ->
%%     [];
distribute(Src_guid_prefix, Src_entity_id, Dst_guid_prefix, Dst_entity_id, Submsg, Reply_to, TS, Endpoints) ->
    Src_guid = guid(Src_guid_prefix, Src_entity_id),
    Dst_guid = guid(Dst_guid_prefix, Dst_entity_id),
    Msg = #msg{src_guid = Src_guid, dst_guid = Dst_guid, submsg = Submsg, reply_to = Reply_to, timestamp = TS},
    [{Pid, Msg} || #endpoint{local_guid = #guid{guid_prefix = Local_guid_prefix, 
						entity_id = Local_entity_id} = Local_guid,
			     remote_guid = Remote_guid, % NB: can be undefined, unknown or a #guid
			     pid = Pid} <- Endpoints,
		   Src_guid_prefix =/= Local_guid_prefix,
		   Remote_guid =/= undefined,
		   Dst_guid =:= Local_guid orelse Dst_guid =:= unknown
		   orelse (Dst_entity_id =:= Local_entity_id andalso
			   Dst_guid_prefix =:= unknown)
		   orelse (Remote_guid =/= unknown andalso 
			   Src_entity_id =:= Remote_guid#guid.entity_id andalso
			   Remote_guid#guid.guid_prefix =:= unknown)
    ].
	
guid(unknown, unknown) ->
    unknown;
guid(Guid_prefix, Entity_id) ->
    #guid{guid_prefix = Guid_prefix, entity_id = Entity_id}.

%% @private

%% @doc This function adds submessage meant for an endpoint to the
%% accumulated submessages for that endpoint. An endpoint is
%% identified by its pid. The submessage is part of the #msg record
%% which holds information relevant for the endpoint besides the
%% submessage itself.
%%
%% The accumulator is an ordered set with the pid of an endpoint as
%% the key. A newly added pid / message combination is turned into an
%% ordered set itself which is merged into the accumulated set. The
%% merge function takes the list of messages associated with a
%% particular pid and appends the new message to it.
%%
%% TODO: Why use orddict? Really can't think why dict isn't more
%% appropriate.
accum([], Acc) ->
    Acc;
accum([{Pid, Msg} | Rest], Acc) ->
    Fun = fun(_Key, Msgs1, Msgs2) ->
		  merge(Msgs1, Msgs2)
	  end,
    Acc1 = orddict:merge(Fun, Acc, orddict:from_list([{Pid, [Msg]}])),
    accum(Rest, Acc1).

%% @private

%% @doc This function merges submessages destined for an
%% endpoint. Currently, the order the submessages are processed by the
%% receiver is kept but one could maybe do something more intelligent
%% here.
merge(Rcvs1, Rcvs2) ->
    Rcvs1 ++ Rcvs2.
